﻿#include "stdafx.h"
#include "RoadTerrian.h"

#include "RoadMileLine.h"
#include "Road3dParamGen.h"
#include "QMessageBox"
#ifdef USE_FADE25D_TERRIAN

int RoadTerrian::InsertConstrained(ConstrainPolyonID& idPolyon, const ZMVector<ZMPoint3D>& vConstrain)
{
	vector<GEOM_FADE25D::Point2> polyon;
	for (ZMPoint3D pnt : vConstrain)
	{
		GEOM_FADE25D::Point2 pntFad(pnt[0], pnt[1], pnt[2]);
		polyon.push_back(pntFad);
	}
	vector<GEOM_FADE25D::Segment2> vSeg2;
	for (int i = 0; i < polyon.size(); ++i)
	{
		vSeg2.push_back(GEOM_FADE25D::Segment2(polyon[i], polyon[(i + 1) % 4]));
	}

	GEOM_FADE25D::ConstraintGraph2* pCG(NULL);
	pCG = m_pTerrain->createConstraint(vSeg2, GEOM_FADE25D::CIS_CONSTRAINED_DELAUNAY);
	m_pTerrain->applyConstraintsAndZones();

	GEOM_FADE25D::Zone2* pZone(m_pTerrain->createZone(pCG, GEOM_FADE25D::ZL_INSIDE));
	m_vContrainZone.push_back(pZone);
	idPolyon = m_vContrainZone.size() - 1;
	return 0;
}

int RoadTerrian::RemoveAllConstrined()
{
	for (GEOM_FADE25D::Zone2* pZone : m_vContrainZone)
	{
		m_pTerrain->deleteZone(pZone);
	}
	m_vContrainZone.clear();
	return 0;
}

int RoadTerrian::RemoveConstrined(ConstrainPolyonID id, bool isNeedUpdate)
{
	m_pTerrain->deleteZone(m_vContrainZone[id]);
	m_vContrainZone.erase(m_vContrainZone.begin() + id);
	return 0;
}

int RoadTerrian::GetHeight(const ZMReal& x, const ZMReal& y, ZMReal& z) const
{
	return m_pTerrain->getHeight(x, y, z) == true ? 0 : 1;
}

void RoadTerrian::LoadData(QString strFileName)
{
	ifstream  fp;
	fp.open(ToString(strFileName).c_str());
	ZMVector<GEOM_FADE25D::Point2> vPnt;
	if (fp.is_open())
	{
		int iNum;
		fp >> iNum;
		vPnt.resize(iNum);
		for (int i = 0; i < iNum; ++i)
		{
			ZMReal x, y, z;
			fp >> x; fp >> y; fp >> z;
			vPnt[i] = GEOM_FADE25D::Point2(x, y, z);
			vPnt[i].setCustomIndex(i);
		}
		fp.close();
		m_pTerrain = SharedPointer<GEOM_FADE25D::Fade_2D>(new GEOM_FADE25D::Fade_2D(vPnt.size()));
		m_pTerrain->insert(vPnt);
	}
}

void RoadTerrian::GetTriangleMeshData(MeshData& meshData)
{
	meshData.SetMeshName("TerrianMesh");
	meshData.SetFlags(MeshData::NOT_SELECT_MASK);
	ZMVector<int> vInx;
	ZMVector<ZMPoint3D> vPnt;
	map<GEOM_FADE25D::Triangle2*, int> mapInfo = GetTriangleDomains();


	vector<GEOM_FADE25D::Point2*> vAllPnts;
	m_pTerrain->getVertexPointers(vAllPnts);
	vPnt.resize(vAllPnts.size());
	for (int i = 0; i < vAllPnts.size(); ++i)
	{
		vAllPnts[i]->setCustomIndex(i);
		vPnt[i] = ZMPoint3D(vAllPnts[i]->x(), vAllPnts[i]->y(), vAllPnts[i]->z());
	}

	vInx.reserve(mapInfo.size());
	for (map<GEOM_FADE25D::Triangle2*, int>::iterator pos = mapInfo.begin(); pos != mapInfo.end(); ++pos)
	{
		if (pos->second == 0)
		{
			GEOM_FADE25D::Triangle2* pTri = pos->first;
			for (int i = 0; i < 3; ++i)
			{
				vInx.push_back(pTri->getCorner(i)->getCustomIndex());
			}
		}
	}

	ZMVector<ZMPoint2D> vUV;
	Plane3dParamGen gen;
	vUV = gen.GenUV(vPnt);
	ZMVector<ZMPoint3D> vNormal, vTangent;
	gen.GenNormalAndTangent(vPnt, vInx, vNormal, vTangent);
	meshData.SetPntData(vPnt);
	meshData.SetTriIndex(vInx);
	meshData.SetNormalData(vNormal);
	meshData.SetTangentData(vTangent);
	meshData.SetUVData(vUV);
}

map<GEOM_FADE25D::Triangle2*, int> RoadTerrian::GetTriangleDomains()
{
	vector<GEOM_FADE25D::Triangle2*> vAllTriangles;
	m_pTerrain->getTrianglePointers(vAllTriangles);
	map<GEOM_FADE25D::Triangle2*, int> vTriInfo;
	for (GEOM_FADE25D::Triangle2* pTri : vAllTriangles)
	{
		vTriInfo[pTri] = 0;
	}

	for (GEOM_FADE25D::Zone2* pZone : m_vContrainZone)
	{
		vector<GEOM_FADE25D::Triangle2*> vContrainTriangles;
		pZone->getTriangles(vContrainTriangles);
		for (GEOM_FADE25D::Triangle2* pTri : vContrainTriangles)
		{
			vTriInfo[pTri] = 1;
		}
	}
	return vTriInfo;
}
#endif
#ifdef USE_CGAL_TERRIAN



#include <CGAL/exceptions.h>
#include <CGAL/Triangle_3.h>
#include "PolyonToTriangle.h"

#ifdef USE_CGAL_INTERSECTION
#include <CGAL/Boolean_set_operations_2.h>
#else
#include "clipper.hpp" 
using namespace ClipperLib;
#endif

#include <CGAL/Boolean_set_operations_2.h>
namespace CGAL
{
	template <class Kernal>
	inline bool do_intersect(const Polygon_2<Kernal>& polyon, const ZMTriangle& tri)
	{
		CGAL::Polygon_2<Kernal>    polyon2;
		for (int i = 0; i < 3; ++i)
		{
			Kernal::Point_2 pnt(tri[i][0], tri[i][1]);
			polyon2.push_back(pnt);
		}
		return CGAL::do_intersect(polyon2, polyon);
	}

	template <class Kernal>
	bool inline do_intersect(const CGAL::Polygon_2<Kernal>& polyon, const CGAL::Bbox_3& c)
		{
			CGAL::Polygon_2<Kernal>    polyon2;
			polyon2.push_back(CGALPoint2(c.xmin(), c.ymin()));
			polyon2.push_back(CGALPoint2(c.xmax(), c.ymin()));
			polyon2.push_back(CGALPoint2(c.xmax(), c.ymax()));
			polyon2.push_back(CGALPoint2(c.xmin(), c.ymax()));
			return CGAL::do_intersect(polyon2, polyon);
		}
};

class FindAllIntersectionObj
{
	typedef typename TreeTri Tree1;
	typedef typename Tree1::Node Node1;
	typedef typename Tree1::AABB_traits::Primitive Primitive1;
	typedef typename Primitive1::Id Iterator_reference1;
	typedef typename Primitive1::Datum Datum1;

	struct suNodeData
	{
		suNodeData(Node1*pNode1, int iPrimitives1)
		{
			m_pNode1 = pNode1;
			m_iPrimitives1 = iPrimitives1;
		}
		Node1* m_pNode1;
		int m_iPrimitives1;
	};
public:
	typedef typename Iterator_reference1 Iterator_reference;
	
	FindAllIntersectionObj(PTreeTri pTri, CGAL::Polygon_2<KCGALWithInfo> * polyon) :
		m_pTri(pTri),
		m_pPolyon(polyon)
	{
		
	}

	vector<Iterator_reference> FindAllInterTri()
	{
		std::vector<Iterator_reference1> lsOutData;
		std::stack<suNodeData> vStack;
		vStack.push(suNodeData(const_cast<Node1*>(m_pTri->root_node()), m_pTri->size()));
		while (!vStack.empty())
		{
			suNodeData nodeData = vStack.top(); vStack.pop();
			Node1* pNode1 = nodeData.m_pNode1;
			int m_iPrimitives1 = nodeData.m_iPrimitives1;
			if (m_iPrimitives1 == 2)
			{
				if (CGAL::do_intersect(*m_pPolyon, pNode1->left_data().datum()))
				{
					lsOutData.push_back(pNode1->left_data().id() );
				}
				if (CGAL::do_intersect(*m_pPolyon,  pNode1->right_data().datum()))
				{
					lsOutData.push_back(pNode1->right_data().id());
				}
			}
			else if (m_iPrimitives1 == 3)
			{
				if (CGAL::do_intersect(*m_pPolyon, pNode1->left_data().datum()))
				{
					lsOutData.push_back(pNode1->left_data().id());
				}
				if (CGAL::do_intersect(*m_pPolyon, pNode1->right_child().bbox()))
				{
					vStack.push(suNodeData(&(pNode1->right_child()), 2));
				}
			}
			else
			{
				if (CGAL::do_intersect(*m_pPolyon, pNode1->right_child().bbox()))
				{
					vStack.push(suNodeData(&(pNode1->right_child()), m_iPrimitives1 -1));
				}
				if (CGAL::do_intersect(*m_pPolyon, pNode1->left_child().bbox()))
				{
					vStack.push(suNodeData(&(pNode1->left_child()), m_iPrimitives1 - 1));
				}

			}

		}
		return lsOutData;
	}
private:
	PTreeTri m_pTri;
	CGAL::Polygon_2<KCGALWithInfo> * m_pPolyon;
};



RoadTerrian::RoadTerrian()
{
}

#ifdef USE_CONSTRAINT_TRIANGLE
	int RoadTerrian::m_idUnitPolyInxMax = 0;
#endif

RoadTerrian::~RoadTerrian()
{
}

void RoadTerrian::LoadDataTemp(QString strFileName, const ZMVector<ZMReal>& vMile, const ZMVector<ZMPoint3D>& vPnt, const ZMVector<ZMReal>& vAngle, PRoadLineDesign pRoadLine)
{
#ifdef USE_DOUBLE
	ZMPoint3D pntMin(XMIN, YMIN, 0);
#else
	ZMPoint3D pntMin(0, 0, 0);
#endif
	RoadMileLine line(vMile,vPnt, vAngle, pRoadLine);
	ZMReal dFrom = vMile[1], dTo = vMile[vPnt.size() - 2];
	ifstream  fp;
	ZMVector<ZMPoint3D> vPntTemp;
	fp.open(ToString(strFileName).c_str());
	if (fp.is_open())
	{
		int iNum;
		fp >> iNum;
		for (int i = 0; i < iNum; ++i)
		{
			ZMReal x, y, z;
			fp >> y; fp >> x; fp >> z;
			ZMPoint3D pnt(x - pntMin[0], y - pntMin[1], z - pntMin[2]);
			ZMReal dMile = line.GetMileValue(pnt);
			if (dMile >= dFrom - 100 && dMile <= dTo + 100)
			{
				vPntTemp.push_back(pnt);
			}
		}
		fp.close();
	}
	SavePntData("e:\\pnt.txt");
}

void RoadTerrian::LoadData(QString strFileName)
{
#ifdef USE_DOUBLE
	ZMPoint3D pntMin(XMIN, YMIN, 0);
#else
	ZMPoint3D pntMin(0, 0, 0);
#endif
	ifstream  fp;
	fp.open(ToString(strFileName).c_str());
	ZMVector<Point> vPnt;
	if (fp.is_open())
	{
		int iNum;
		fp >> iNum;
		vPnt.resize(iNum);
		for (int i=0; i<iNum; ++i)
		{
			ZMReal x, y, z;
			fp >> x;fp >> y; fp >> z;
			vPnt[i] = Point(x - pntMin[0], y - pntMin[1]);
			vPnt[i].Index() = i;
			vPnt[i].z() = z - -pntMin[2];
			
		}
		fp.close();
		ClearTriData();

		m_Triangle.insert(vPnt.begin(), vPnt.end());
	}
#ifndef USE_CONSTRAINT_TRIANGLE
	UpdateTreeData();
#endif
}

void RoadTerrian::SavePntData(QString strFileName) 
{
	ofstream  fp;
	fp.open(ToString(strFileName).c_str());
	if (fp.is_open())
	{
		fp << GetPntNum();
		for (CDTP::Finite_vertices_iterator pos = m_Triangle.finite_vertices_begin(); pos != m_Triangle.finite_vertices_end(); ++pos)
		{
			CDTP::Point pnt = pos->point();
			char buffer[1024];
			sprintf_s(buffer, "%f	%f	%f\n", pnt[0], pnt[1], pnt[2]);
			fp << buffer;

		}
		fp.close();
	}
}

//int RoadTerrian::ContructTriangle()
//{
//	ClearTriData();
//
//	m_Triangle.insert(m_vPnt.begin(), m_vPnt.end());
//	ZMVector<Point> vContrianPnt;
//	for (int i=0; i<m_vContrainPnt.size(); ++i)
//	{
//		m_vConstraintID.push_back(m_Triangle.insert_constraint(m_vContrainPnt[i].begin(), m_vContrainPnt[i].end()));
//	}
//	return 0;
//}

#ifndef USE_CGAL_INTERSECTION
Path RoadTerrian::ToClipperPath(const Polygon_2& poly1, ZMReal fScale, ZMReal xMin, ZMReal yMin)
{
	Path ret;
	for (Polygon_2::Vertex_iterator pos = poly1.vertices_begin(); pos != poly1.vertices_end(); ++pos)
	{
		int x = (int)((pos->x() - xMin) * fScale);
		int y = (int)((pos->y() - yMin)* fScale);
		int z = (int)pos->z();
		ret.push_back(IntPoint(x, y, z, pos->Index()));
	}

	if (Orientation(ret) == false)
	{
		std::reverse(ret.begin(), ret.end());
	}
	return ret;
}

CGALPoint RoadTerrian::IntPntToCGALPnt(const IntPoint& pnt, ZMReal fScale, ZMReal xMin, ZMReal yMin)
{
	ZMReal x = xMin + pnt.X / fScale;
	ZMReal y = yMin + pnt.Y / fScale;
	ZMReal z = pnt.Z;
	CGALPoint ret(x,y, z);
	ret.Index() = pnt.m_inx;
	return ret;
}

RoadTerrian::Polygon_2 RoadTerrian::ToPolyon(const ClipperLib::Path& poly, ZMReal fScale, ZMReal xMin, ZMReal yMin)
{
	Polygon_2 ret;
	for (IntPoint pntPoly : poly)
	{
		ZMReal x = xMin + pntPoly.X / fScale;
		ZMReal y = yMin + pntPoly.Y / fScale;
		ZMReal z =  pntPoly.Z;
		CGALPoint2 pnt(x, y);
		pnt.z() = z;
		pnt.Index() = pntPoly.m_inx;
		ret.push_back(pnt);
	}
	return ret;
}

int RoadTerrian::ToClipperPaths(ClipperLib::Paths& vPoly, const ZMVector<CGALPoint>&vPnt, const ZMVector<int>& vInx, ZMReal fScale, ZMReal xMin, ZMReal yMin)
{
	vPoly.reserve(vInx.size() / 3);
	for (int i=0; i<vInx.size(); i+=3)
	{
		Path poly;
		for (int j=0; j<3; ++j)
		{
			CGALPoint pnt = vPnt[vInx[i + j]];
			int x = (int)((pnt[0] - xMin) * fScale);
			int y = (int)((pnt[1] - yMin)* fScale);
			int z = (int)pnt[2];
			int inx = pnt.Index();
			poly.push_back(IntPoint(x, y, z, inx));
		}

		/*if (Orientation(poly) == false)
		{
			std::reverse(poly.begin(), poly.end());
		}*/
		vPoly.push_back(poly);
	}
	return 0;
}
#endif

bool RoadTerrian::DoIntersection(const Polygon_2& poly1, const Polygon_2& poly2)
{
#ifdef USE_CGAL_INTERSECTION
	return CGAL::do_intersect(poly1, poly2);
#else
	ZMReal fScale = 1e+6;
	ZMReal xMin = poly1.vertex(0)[0], yMin = poly1.vertex(1)[1];
	for (int i=0; i<poly1.size(); ++i)
	{
		xMin = min(poly1.vertex(i)[0], xMin);
		yMin = min(poly1.vertex(i)[1], yMin);
	}
	for (int i = 0; i < poly2.size(); ++i)
	{
		xMin = min(poly2.vertex(i)[0], xMin);
		yMin = min(poly2.vertex(i)[1], yMin);
	}
	Path poly[2] = {ToClipperPath(poly1,fScale,  xMin, yMin), ToClipperPath(poly2, fScale, xMin, yMin)};
	Clipper clpr;
	Paths poly0;
	clpr.AddPath(poly[0], ptSubject, true);
	clpr.AddPath(poly[1], ptClip, true);
	clpr.Execute(ctIntersection, poly0, pftEvenOdd, pftEvenOdd);
	return poly0.size() > 0;
#endif
}

ZMVector<RoadTerrian::Polygon_2> RoadTerrian::SimplePolygon(const Polygon_2& poly, bool bUseNewData)
{
	ZMReal fScale = 1e+6;
	ZMReal xMin = poly.vertex(0)[0], yMin = poly.vertex(1)[1];
	map<int, ZMPoint3D> vPntData;
	Polygon_2 polyTemp = poly;
	int inxPnt = 0;
	for (int i = 0; i < polyTemp.size(); ++i)
	{
		(polyTemp.vertices_begin() + i)->Index() = inxPnt++;
		vPntData[inxPnt - 1] = ZMPoint3D(*(polyTemp.vertices_begin() + i));
	}

	Path poly0 = ToClipperPath(polyTemp, fScale, xMin, yMin);
	Paths vSimplePoly;
	ZMReal fAreaTotal = Area(poly0);
	
	SimplifyPolygon(poly0, vSimplePoly);
	ZMVector<RoadTerrian::Polygon_2> ret;
	for (Path poly: vSimplePoly)
	{
		if (IsValidPolyon(fScale, poly) )
		{
			ret.push_back(ToPolyon(poly, fScale, xMin, yMin));
		}
	}
	if (!bUseNewData)
	{
		ZMVector<Polygon_2> retTemp = ret;
		ret.clear();
		for (Polygon_2& polyTemp : retTemp)
		{
			int i = 0;
			for (i = 0; i < polyTemp.size(); ++i)
			{
				int inx = (polyTemp.vertices_begin() + i)->Index();
				if (vPntData.find(inx) != vPntData.end())
				{
					ZMPoint3D pnt = vPntData[inx];
					CGALPoint2 pnt2(pnt[0], pnt[1]);
					pnt2.z() = pnt[2];
					pnt2.Index() = inx;
					*(polyTemp.vertices_begin() + i) = pnt2;
				}
				else
				{
					ZMPoint3D pnt(*(polyTemp.vertices_begin() + i));
					CGALPoint2 pnt2(pnt[0], pnt[1]);
					ZMReal z;
					GetHeight(pnt[0], pnt[1], z);
					pnt2.z() = z;
					pnt2.Index() = -100;
					*(polyTemp.vertices_begin() + i) = pnt2;
				}
			}
			ret.push_back(polyTemp);
		}
	}
	else
	{
		for (Polygon_2& polyTemp : ret)
		{
			int i = 0;
			for (i = 0; i < polyTemp.size(); ++i)
			{
				int inx = (polyTemp.vertices_begin() + i)->Index();
				if (vPntData.find(inx) == vPntData.end())
				{
					ZMPoint3D pnt(*(polyTemp.vertices_begin() + i));
					CGALPoint2 pnt2(pnt[0], pnt[1]);
					ZMReal z;
					GetHeight(pnt[0], pnt[1], z);
					pnt2.z() = z;
					pnt2.Index() = -100;
					*(polyTemp.vertices_begin() + i) = pnt2;
				}
			}
		}
	}

#ifdef DEBUG
	{
		CFileSaveData fp("PolyNeedSample.scr");
		WritePolyon(poly, fp);
	}
	{
		CFileSaveData fp("PolyAfterSample.scr");
		for (Polygon_2& polyTemp : ret)
		{
			WritePolyon(polyTemp, fp);
			fp << "\r\n";

		}
	}
#endif
	return ret;
}

ZMVector<RoadTerrian::Polygon_2> RoadTerrian::IntersectionUnit(const Polygon_2& poly, const ZMVector<Polygon_2 >& vPoly)
{
	ZMReal fScale = 1e+6;
	ZMReal fScale2 = fScale*fScale;
	ZMVector<Polygon_2> ret;
#ifdef USE_CGAL_INTERSECTION
	assert(false)
#else
	ZMVector<Polygon_2> vPolyUnit;
	vPolyUnit.push_back(poly);
	vPolyUnit.insert(vPolyUnit.end(), vPoly.begin(), vPoly.end());
	ZMReal xMin = poly.vertex(0)[0], yMin = poly.vertex(1)[1];
	for (Polygon_2 polyTemp: vPolyUnit)
	{
		for (int i = 0; i < polyTemp.size(); ++i)
		{
			xMin = min(polyTemp.vertex(i)[0], xMin);
			yMin = min(polyTemp.vertex(i)[1], yMin);
		}
	}

	map<int, ZMPoint3D> vPntData;
	int inxPnt = 0;
	for (Polygon_2& polyTemp : vPolyUnit)
	{
		for (int i=0; i<polyTemp.size(); ++i)
		{
			(polyTemp.vertices_begin() +i)->Index() = inxPnt++;
			vPntData[inxPnt - 1] = ZMPoint3D(*(polyTemp.vertices_begin() + i));
		}
	}

	Paths poly0;
	poly0.push_back(ToClipperPath(vPolyUnit[0], fScale, xMin, yMin));
	Paths poly1;
	for (int i=1; i<vPolyUnit.size(); ++i)
	{
		poly1.push_back(ToClipperPath(vPolyUnit[i], fScale, xMin, yMin));
	}
	Clipper clpr;
	clpr.AddPaths(poly0, ptSubject, true);
	clpr.AddPaths(poly1, ptClip, true);
	Paths polyUnit;
	clpr.Execute(ctUnion, polyUnit, pftEvenOdd, pftEvenOdd);
	assert(polyUnit.size() > 0);
	
	for (int i=0; i<polyUnit.size(); ++i)
	{
		Paths vSimplePoly;
		ZMReal fAreaTotal = Area(polyUnit[i]);
		SimplifyPolygon(polyUnit[i], vSimplePoly);
		for (Path simplePoly: vSimplePoly)
		{
			if (IsValidPolyon(fScale, simplePoly))
			{
				ret.push_back(ToPolyon(simplePoly, fScale, xMin, yMin));
			}
		}
		
	}


	ZMVector<Polygon_2> retTemp = ret;
	ret.clear();
	for (Polygon_2& polyTemp : retTemp)
	{
		int i = 0;
		for ( i = 0; i<polyTemp.size(); ++i)
		{
			int inx = (polyTemp.vertices_begin() + i)->Index();
			if (vPntData.find(inx) != vPntData.end())
			{
				ZMPoint3D pnt = vPntData[inx];
				CGALPoint2 pnt2(pnt[0], pnt[1]);
				pnt2.z() = pnt[2];
				pnt2.Index() = inx;
				*(polyTemp.vertices_begin() + i) = pnt2;
			}
			else
			{
				ZMPoint3D pnt(*(polyTemp.vertices_begin() + i));
				CGALPoint2 pnt2(pnt[0], pnt[1]);
				ZMReal z;
				GetHeight(pnt[0], pnt[1], z);
				pnt2.z() = z;
				pnt2.Index() = -100;
				*(polyTemp.vertices_begin() + i) = pnt2;
			}
		}
		ret.push_back(polyTemp);
	}
#ifdef DEBUG
	{
		CFileSaveData fp("HolePoly\\HolePolyTotal.scr");
		for (Polygon_2 poly:ret)
		{
			fp << "3dpoly ";
			for (int j=0; j< poly.size() ;++j)
			{
				ZMPoint3D pnt (poly.vertex(j));
				fp << pnt.x() << ",";		fp << pnt.y() << ",";		fp << pnt.z() << "\r\n";
			}
			fp << "\r\n";
		}
		
	}
	
#endif
#endif
	return ret;
}

bool RoadTerrian::IsValidPolyon(ZMReal fScale, const Path& polyon)
{
	ZMReal fScale2 = fScale*fScale;
	ZMReal fArea = fabs(Area(polyon));
	ZMReal fGirth = Girth(polyon);
	
	if (fArea > fScale2 / 2.0f && fArea / fGirth > fScale * 0.3)
	{
		return true;
	}
	return false;
}


#ifdef USE_CONSTRAINT_TRIANGLE
RoadTerrian::ConstrainPolyonID RoadTerrian::AddConstrainPoly(int idRoad, const Polygon_2& vConstrain)
{
#ifdef DEBUG_ADD_CONSTRAIN
	WriteDataToCAD("beginInsert.scr");
#endif
	if (m_vConstrainPolyGroup.find(idRoad) == m_vConstrainPolyGroup.end())
	{
		m_vConstrainPolyGroup[idRoad] = ZMVector<suConstrainPolyInfo>();
	}
	suConstrainPolyInfo infoPolyAdd;
	infoPolyAdd.m_idConstrain.m_idRoad = idRoad;
	infoPolyAdd.m_idConstrain.m_inxContrain = m_vConstrainPolyGroup[idRoad].size();
	infoPolyAdd.m_idPolyUnit = -1;
	infoPolyAdd.m_poly = vConstrain;
	                      
	set<ConstrainPolyonID> vPolyIDNeedUnit;
	for (map<int, ZMVector<suConstrainPolyInfo>>::iterator pos = m_vConstrainPolyGroup.begin(); 
		pos != m_vConstrainPolyGroup.end(); ++pos) 
	{
		if (pos->first != idRoad)
		{
			for (suConstrainPolyInfo& info : pos->second)
			{
				if (DoIntersection(info.m_poly, infoPolyAdd.m_poly))
				{
					vPolyIDNeedUnit.insert(info.m_idConstrain);
				}
			}
		}
	}
		
	ZMVector<ConstrainPolyonID> vRelationID;
	for (ConstrainPolyonID id: vPolyIDNeedUnit)
	{
		ZMVector<ConstrainPolyonID> vRelationIDTemp = FindRelationPolyonID(id);
		vRelationID.insert(vRelationID.end(), vRelationIDTemp.begin(), vRelationIDTemp.end());
	}
	vPolyIDNeedUnit.insert(vRelationID.begin(), vRelationID.end());

	set<int> vConstrainNeedRemove;
	for (ConstrainPolyonID id : vPolyIDNeedUnit)
	{
		vConstrainNeedRemove.insert(GetPolyInfo(id).m_idPolyUnit);
	}
	for (int inxNeedRemove: vConstrainNeedRemove)
	{
		RemoveConstrined(inxNeedRemove, false);
	}
#ifdef DEBUG_ADD_CONSTRAIN
	WriteDataToCAD("afterRemove.scr");
#endif
	Polygon_2 ployUnit = infoPolyAdd.m_poly;
	ZMVector<Polygon_2> vPolyNeedUnit;
	for (ConstrainPolyonID id : vPolyIDNeedUnit)
	{
		vPolyNeedUnit.push_back(GetPolyInfo(id).m_poly);
	}
	if (vPolyIDNeedUnit.size() > 0)
	{
		vPolyNeedUnit = IntersectionUnit(ployUnit, vPolyNeedUnit);
	}
	else
	{
		if (ployUnit.is_simple())
		{
			vPolyNeedUnit.push_back(ployUnit);
		}
		else
		{
#ifdef DEBUG
			{
				CFileSaveData fp("SimpleBefore.scr");
				fp << "3dpoly ";
				for (Polygon_2::iterator pos = ployUnit.vertices_begin(); pos != ployUnit.vertices_end(); ++pos)
				{
					fp << pos->x() << ",";		fp << pos->y() << ",";		fp << pos->z() << "\r\n";
				}
				fp << "\r\n";
			}
#endif
			vPolyNeedUnit = SimplePolygon(ployUnit);

			for (int i=0; i<vPolyNeedUnit.size(); ++i)
			{
#ifdef DEBUG
				if (!vPolyNeedUnit[i].is_simple())
				{
					{
						CFileSaveData fp("SimpleBefore.scr");
						fp << "3dpoly ";
						for (Polygon_2::iterator pos = ployUnit.vertices_begin(); pos != ployUnit.vertices_end(); ++pos)
						{
							fp << pos->x() << ",";		fp << pos->y() << ",";		fp << pos->z() << "\r\n";
						}
						fp << "\r\n";
					}

					{
						CFileSaveData fp("SimpleEnd.scr");
						fp << "3dpoly ";
						for (Polygon_2::iterator pos = vPolyNeedUnit[i].vertices_begin(); pos != vPolyNeedUnit[i].vertices_end(); ++pos)
						{
							fp << pos->x() << ",";		fp << pos->y() << ",";		fp << pos->z() << "\r\n";
						}
						fp << "\r\n";
					}
				}
#endif

				if (!vPolyNeedUnit[i].is_simple())
				{
					vPolyNeedUnit = SimplePolygon(ployUnit, true);
				}
				assert(vPolyNeedUnit[i].is_simple());
			}

		}
	}
	suUnitPolyInfo unitInfo;

	int inxPolyon = 0;
	for (Polygon_2 poly : vPolyNeedUnit)
	{
//#define INSERT_AS_POLY
#ifdef INSERT_AS_POLY
		unitInfo.m_idConstrain.push_back(m_Triangle.insert_constraint(poly));
#else
		bool isPolyonOk = false;
		CDTP triBack;
		triBack.copy_triangulation(m_Triangle);
		try
		{
			unitInfo.m_idConstrain.push_back(m_Triangle.insert_constraint(poly));
			isPolyonOk = true;
		}
		catch (CGAL::Failure_exception& e)
		{
			isPolyonOk = false;
		}
		if (isPolyonOk == false)
		{
			m_Triangle = CDTP();
			m_Triangle.copy_triangulation(triBack);
			ZMVector<CGALPoint2> vPnt;
			for (Polygon_2::Vertex_iterator pos = poly.vertices_begin(); pos != poly.vertices_end(); ++pos)
			{
				vPnt.push_back(*pos);
			}

			CGALPoint2 pntCurrent;
			if (!vPnt.empty())
			{
				pntCurrent = vPnt[0];
			}
			for (int i = 0; i < vPnt.size(); ++i)
			{
				CGALPoint2 pntNext = vPnt[(i + 1) % vPnt.size()];
				try
				{
					unitInfo.m_idConstrain.push_back(m_Triangle.insert_constraint(pntCurrent, pntNext));
					pntCurrent = pntNext;
				}
				catch (CGAL::Failure_exception& e)
				{
					WriteDataToCAD("CurrentError.scr", ZMPoint3D(pntCurrent), 200);

					{
						CFileSaveData fp("pointData.scr");
						fp << "3dpoly ";
						fp << pntCurrent.x() << ",";		fp << pntCurrent.y() << ",";		fp << pntCurrent.z() << "\r\n";
						fp << pntNext.x() << ",";		fp << pntNext.y() << ",";		fp << pntNext.z() << "\r\n";
						fp << "\r\n";
					}
					QMessageBox::about(NULL, "", ToQString("错误%1, %2").arg(inxPolyon).arg(i));
					int k = 0;
				}

			}
			++inxPolyon;
		}
		
#endif // INSERT_POLY

		
	}
	m_FaceCatch = CDTP::Face_handle();

#ifdef DEBUG_ADD_CONSTRAIN
	WriteDataToCAD("afterInsert.scr");
#endif	
	unitInfo.m_vInxPolyInfo.insert(unitInfo.m_vInxPolyInfo.end(), vPolyIDNeedUnit.begin(), vPolyIDNeedUnit.end());
	unitInfo.m_vInxPolyInfo.push_back(infoPolyAdd.m_idConstrain);
	unitInfo.m_idPolyUnit = ++m_idUnitPolyInxMax;
	infoPolyAdd.m_idPolyUnit = unitInfo.m_idPolyUnit;
	for (ConstrainPolyonID id : vPolyIDNeedUnit)
	{
		GetPolyInfo(id).m_idPolyUnit = unitInfo.m_idPolyUnit;
	}
	m_mapUnitAndID[unitInfo.m_idPolyUnit] = unitInfo;
	m_vConstrainPolyGroup[idRoad].push_back(infoPolyAdd);
	return infoPolyAdd.m_idConstrain;
}

int RoadTerrian::InsertConstrained(ConstrainPolyonID& idPolyon, int inxRoad, const ZMVector<ZMPoint3D>& vConstrain, bool isNeedUpdate)
{
#ifdef _DEBUG
	{
		CFileSaveData fp("polyConstrained.scr");
		fp << "3dpoly ";
		for (ZMPoint3D pnt : vConstrain)
		{
			fp << pnt.x() << ",";		fp << pnt.y() << ",";		fp << pnt.z() << "\r\n";
			assert(pnt.z() > 100);
		}
		fp << "\r\n";
	}

#endif
	ZMVector<Point>  vPnt = ToCGALPntData(vConstrain);
	if (CGAL::squared_distance(vPnt[0], vPnt[vPnt.size() - 1]) < 0.01*0.01)
	{
		vPnt.pop_back();
	}
	Polygon_2 poly;
	for (Point pnt : vPnt)
	{
		poly.push_back(pnt);
	}

//	assert(poly.is_simple());
	
	CStreamSaveData fpSave;
	int ret = 1;
	try
	{
		//SaveBinaray(fpSave);
		idPolyon = AddConstrainPoly(inxRoad, poly);
		m_FaceCatch = CDTP::Face_handle();
		if (isNeedUpdate)
		{
			UpdatePntAndTriData();
		}
		ret = 0;
	}
	catch (CGAL::Failure_exception& e)
	{
		//ZMVector<unsigned char>  vData = fpSave.GetBinaryData();
		//CStreamLoadData fpLoad(vData.data(), vData.size());
		//LoadBinaray(fpLoad);
		int k = 0;

	}
	return ret;
}
#endif

void RoadTerrian::UpdatePntInx()
{
	int i = 0, inxMax;
	for (CDTP::Finite_vertices_iterator pos = m_Triangle.finite_vertices_begin(); pos != m_Triangle.finite_vertices_end(); ++pos)
	{
		if (pos->point().Index() >=0)
		{
			pos->point().Index() = i;
			i = i + 1;
		}
	}

#ifndef USE_CONSTRAINT_TRIANGLE
	inxMax = i;
	for (Polygon_2& poly : m_vContrainPoly)
	{
		for (Polygon_2::Vertex_iterator pos = poly.vertices_begin(); pos != poly.vertices_end(); ++pos)
		{
			pos->Index() = inxMax;
			++inxMax;
		}
	}
#endif
	/*for (i=0; i<m_vPnt.size(); ++i)
	{
		m_vPnt[i].Index() = i;
	}
	inxMax = m_vPnt.size();
	for (ZMVector<Point>& vPnt: m_vContrainPnt)
	{
		for (Point& pnt :vPnt)
		{
			pnt.Index() = inxMax++;
		}
	}*/
}

void RoadTerrian::mark_domains(CDTP::Face_handle start,	int index,	std::list<CDTP::Edge>& border)
{
	if (start->info().nesting_level != -1)
	{
		return;
	}
	std::list<CDTP::Face_handle> queue;
	queue.push_back(start);
	while (!queue.empty())
	{
		CDTP::Face_handle fh = queue.front();
		queue.pop_front();
		if (fh->info().nesting_level == -1) 
		{
			fh->info().nesting_level = index;
			for (int i = 0; i < 3; i++) 
			{
				CDTP::Edge e(fh, i);
				CDTP::Face_handle n = fh->neighbor(i);
				if (n->info().nesting_level == -1)
				{
					if (m_Triangle.is_constrained(e)) border.push_back(e);
					else queue.push_back(n);
				}
			}
		}
	}
}

void RoadTerrian::UpdateTriangleDomains()
{
	for (CDTP::All_faces_iterator it = m_Triangle.all_faces_begin(); it != m_Triangle.all_faces_end(); ++it) 
	{
		it->info().nesting_level = -1;
	}
	std::list<CDT::Edge> border;
	mark_domains(m_Triangle.infinite_face(), 0, border);
	while (!border.empty()) 
	{
		CDTP::Edge e = border.front();
		border.pop_front();
		CDTP::Face_handle n = e.first->neighbor(e.second);
		if (n->info().nesting_level == -1) {
			mark_domains(n, e.first->info().nesting_level + 1, border);
		}
	}
}

int RoadTerrian::RemoveAllConstrined()
{
#ifdef USE_CONSTRAINT_TRIANGLE
	for (map<int, suUnitPolyInfo>::iterator pos = m_mapUnitAndID.begin(); pos != m_mapUnitAndID.end(); ++pos)
	{
		for (CDTP::Constraint_id cID : pos->second.m_idConstrain)
		{
			m_Triangle.remove_constraint(cID);
		}
	}
	m_mapUnitAndID.clear();
	m_vConstrainPolyGroup.clear();
#else
	m_vContrainPoly.clear();
#endif

	m_FaceCatch = CDTP::Face_handle();
	return 0;
}

int RoadTerrian::RemoveConstrined(ConstrainPolyonID id, bool isNeedUpdate)
{
#ifdef USE_CONSTRAINT_TRIANGLE
	ZMVector<ConstrainPolyonID>  vRelation = FindRelationPolyonID(id);
	vRelation.push_back(id);
	set<int> vConstrainNeedRemove;
	for (ConstrainPolyonID id : vRelation)
	{
		vConstrainNeedRemove.insert(GetPolyInfo(id).m_idPolyUnit);
	}
	for (int inxNeedRemove : vConstrainNeedRemove)
	{
		RemoveConstrined(inxNeedRemove);
	}
#else
	m_vContrainPoly.erase(m_vContrainPoly.begin() + id);
#endif
	return 0;
}


#ifdef USE_CONSTRAINT_TRIANGLE
int RoadTerrian::RemoveConstrined(int inx, bool isNeedUpdate)
{
	int iErr = 1;
	if (m_mapUnitAndID.find(inx) != m_mapUnitAndID.end())
	{
		iErr = 0;
		for (CDTP::Constraint_id id : m_mapUnitAndID[inx].m_idConstrain)
		{
			m_Triangle.remove_constraint(id);
		}
		m_mapUnitAndID.erase(inx);
		if (isNeedUpdate)
		{
			UpdatePntAndTriData();
		}
		m_FaceCatch = CDTP::Face_handle();


	}

	return iErr;

}
#endif
#ifndef USE_CONSTRAINT_TRIANGLE

void RoadTerrian::UpdateTreeData()
{
	m_pTree = PTreeTri(new TreeTri());
	m_vTri.clear();
	UpdatePntInx();
	for (CDTP::Finite_faces_iterator pos = m_Triangle.finite_faces_begin(); pos != m_Triangle.finite_faces_end(); ++pos)
	{
		CGALPoint pnt[3];
		for (int i=0; i<3; ++i)
		{
			pnt[i] = pos->vertex(i)->point();
			pnt[i] = CGALPoint(pnt[i][0], pnt[i][1], 0);
		}
		CGALTriangle tri(pnt[0], pnt[1], pnt[2]);
		m_vTri.push_back(tri);
		
	}
	m_pTree->insert(m_vTri.begin(), m_vTri.end());
	m_pTree->accelerate_distance_queries();
}
#endif

void RoadTerrian::GetOriginMeshData(ZMVector<CGALPoint>&vPnt, ZMVector<int>& vInx)
{
	
	vInx.reserve(m_Triangle.number_of_faces() * 3);
	for (CDTP::Finite_faces_iterator it = m_Triangle.finite_faces_begin(); it != m_Triangle.finite_faces_end(); ++it)
	{
		for (int i = 0; i < 3; ++i)
		{
			vInx.push_back(it->vertex(i)->point().Index());
		}
	}

	vPnt.reserve(m_Triangle.number_of_vertices());
	for (CDTP::Finite_vertices_iterator pos = m_Triangle.finite_vertices_begin(); pos != m_Triangle.finite_vertices_end(); ++pos)
	{
		if (pos->point().Index() >= 0)
		{
			vPnt.push_back(pos->point());
		}
	}
}

void RoadTerrian::GetTriangleMeshData(MeshData& meshData)
{
	meshData.SetMeshName("TerrianMesh");
	meshData.SetFlags(MeshData::NOT_SELECT_MASK);
	ZMVector<int> vInx;
	ZMVector<ZMPoint3D> vPnt;

#ifdef USE_CONSTRAINT_TRIANGLE

	vInx.reserve(m_Triangle.number_of_faces() * 3);
	for (CDTP::Finite_faces_iterator it = m_Triangle.finite_faces_begin(); it != m_Triangle.finite_faces_end(); ++it)
	{
		if (it->info().nesting_level % 2 == 0)
		{
			bool isValid = true;
			for (int i = 0; i < 3; ++i)
			{
				if (it->vertex(i)->point().Index() < 0)
				{
					isValid = false;
					break;
				}
			}
			if (isValid)
			{
				for (int i = 0; i < 3; ++i)
				{
					vInx.push_back(it->vertex(i)->point().Index());
				}
			}
		}
	}

	vPnt.reserve(m_Triangle.number_of_vertices());
	for (CDTP::Finite_vertices_iterator pos = m_Triangle.finite_vertices_begin(); pos != m_Triangle.finite_vertices_end(); ++pos)
	{
		if (pos->point().Index() >= 0)
		{
			vPnt.push_back(ZMPoint3D(pos->point()));
		}
	}
#else
	UpdateMeshData(vPnt, vInx);
#endif
	ZMVector<ZMPoint2D> vUV;
	Plane3dParamGen gen;
	vUV = gen.GenUV(vPnt);
	ZMVector<ZMPoint3D> vNormal, vTangent;
	gen.GenNormalAndTangent(vPnt, vInx, vNormal, vTangent);
	meshData.SetPntData(vPnt);
	meshData.SetTriIndex(vInx);
	meshData.SetNormalData(vNormal);
	meshData.SetTangentData(vTangent);
	meshData.SetUVData(vUV);
}

int RoadTerrian::GetHeight(const ZMReal& x, const ZMReal& y, ZMReal& z) const
{
	int iErr = 1;
	//if (fabs(x - 506411.3653778) < 0.01 && fabs(y - 3241647.642538) < 0.01)
	//{
	//	if (m_FaceCatch != CDTP::Face_handle())
	//	{
	//		bool isFind = false;
	//		for (CDTP::Finite_faces_iterator pos = m_Triangle.finite_faces_begin(); pos != m_Triangle.finite_faces_end(); ++pos)
	//		{
	//			int i;
	//			for (i = 0; i < 3; ++i)
	//			{
	//				CGALPoint pnt0, pnt1;
	//				pnt0 = pos->vertex(i)->point();
	//				pnt1 = m_FaceCatch->vertex(i)->point();
	//				if (CGAL::squared_distance(pnt0, pnt1) > 0.01)
	//				{
	//					break;
	//				}
	//			}
	//			if (i == 3)
	//			{
	//				isFind = true;
	//				break;
	//			}
	//		}
	//		if (!isFind)
	//		{
	//			bool isIninte = m_FaceCatch->has_vertex(m_Triangle.infinite_vertex());

	//			for (CDTP::Face_iterator pos = m_Triangle.faces_begin(); pos != m_Triangle.faces_end(); ++pos)
	//			{
	//				int i;
	//				for (i = 0; i < 3; ++i)
	//				{
	//					CGALPoint pnt0, pnt1;
	//					if (pos->vertex(i)->is_valid())
	//					{
	//						pnt0 = pos->vertex(i)->point();
	//						pnt1 = m_FaceCatch->vertex(i)->point();
	//						if (CGAL::squared_distance(pnt0, pnt1) > 0.01)
	//						{
	//							break;
	//						}
	//					}
	//				
	//				}
	//				if (i == 3)
	//				{
	//					isFind = true;
	//					break;
	//				}
	//			}
	//		}
	//	}
	//}

	/*{
		CDTP::Face_handle fInfi = m_Triangle.infinite_face();
		CDTP::Vertex_handle v = m_Triangle.infinite_vertex();
		int inx = fInfi->index(v);
		fInfi->neighbor(inx);
	}*/

	CDTP::Face_handle f = m_Triangle.locate(Point(x, y), m_FaceCatch);


	if (f != m_FaceCatch)
	{
		const_cast<RoadTerrian*>(this)->m_FaceCatch = f;
	}
	Point p[3];
	if (f->is_valid()&&!f->has_vertex(m_Triangle.infinite_vertex()))
	{
		iErr = 0;

		for (int i = 0; i < 3; ++i)
		{
			p[i] = f->vertex(i)->point();
		}
		ZMReal w[3];
		ZMReal wTemp = ((p[0].y() - p[2].y())*(p[1].x() - p[2].x()) + (p[1].y() - p[2].y())*(p[2].x() - p[0].x()));
		w[0] = ((y - p[2].y())*(p[1].x() - p[2].x()) + (p[1].y() - p[2].y())*(p[2].x() - x));
		w[0] /= wTemp;

		w[1] = ((y - p[0].y())*(p[2].x() - p[0].x()) + (p[2].y() - p[0].y())*(p[0].x() - x));
		w[1] /= wTemp;

		w[2] = ((y - p[1].y())*(p[0].x() - p[1].x()) + (p[0].y() - p[1].y())*(p[1].x() - x));
		w[2] /= wTemp;

		z = w[0] * p[0].z() + w[1] * p[1].z() + w[2] * p[2].z();
	}
	return iErr;
}

#ifndef USE_CONSTRAINT_TRIANGLE
void RoadTerrian::UpdateMeshData(ZMVector<ZMPoint3D>&vPnt, ZMVector<int>& vInx)
{

	ZMVector<CGALPoint> vTerrianPnt;
	ZMVector<int> vTerrianInx;
	GetOriginMeshData(vTerrianPnt, vTerrianInx);
	ClipperLib::Paths vTerrianPoly;
	
	set<int> vIntTriInx;
	ClipperLib::Paths vPolyonData;
	for (Polygon_2& poly : m_vContrainPoly)
	{
		FindAllIntersectionObj obj(m_pTree, &poly);
		vector<FindAllIntersectionObj::Iterator_reference> vPosTri = obj.FindAllInterTri();
		for (FindAllIntersectionObj::Iterator_reference pos : vPosTri)
		{
			vIntTriInx.insert(pos - m_vTri.begin());
		}
		vPolyonData.push_back(ToClipperPath(poly));
	}

	ToClipperPaths(vTerrianPoly, vTerrianPnt, vTerrianInx);

	Paths poly0;
	for (int inxTri: vIntTriInx)
	{
		Paths poly;
		Clipper clpr;
		clpr.AddPath(vTerrianPoly[inxTri], ptSubject, true);
		clpr.AddPaths(vPolyonData, ptClip, true);
		clpr.Execute(ctDifference, poly, pftNonZero, pftNonZero);
		poly0.insert(poly0.end(), poly.begin(), poly.end());
	}



	vPnt.reserve(vTerrianPnt.size());
	for (const CGALPoint& pnt: vTerrianPnt)
	{
		vPnt.push_back(ZMPoint3D(pnt));
	};
	for (const Polygon_2& poly : m_vContrainPoly)
	{
		for (Polygon_2::Vertex_iterator pos = poly.vertices_begin(); pos != poly.vertices_end(); ++pos)
		{
			vPnt.push_back(ZMPoint3D(*pos));
		}
	}

	for (Path& poly : poly0)
	{
		for (IntPoint& intPnt : poly)
		{
			if (intPnt.m_inx == -1)
			{
				CGALPoint pnt = IntPntToCGALPnt(intPnt);
				ZMReal z;
				GetHeight(pnt[0], pnt[1], z);
				vPnt.push_back(ZMPoint3D(pnt[0], pnt[1], z));
				intPnt.m_inx = vPnt.size() - 1;
			}
		}
	}
	
	for (Path& poly : poly0)
	{
		if (poly.size() == 3)
		{
			for (IntPoint& intPnt : poly)
			{
				vInx.push_back(intPnt.m_inx);
			}
		}
		else if(poly.size() > 3)
		{
			ZMVector<ZMPoint3D> vPolyonTemp;
			ZMVector<int> vPolyonInx;
			for (IntPoint& intPnt : poly)
			{
				vPolyonTemp.push_back(vPnt[intPnt.m_inx]);
			}
			ZMPolyonToTriangle::ToTriangle(vPolyonTemp, vPolyonInx);
			for (int inxPoly : vPolyonInx)
			{
				vInx.push_back(poly[inxPoly].m_inx);
			}
		}
	}

	//插入不属于这个范围的三角形
	set<int>::iterator pos = vIntTriInx.begin();
	for (int i=0; i<vTerrianInx.size(); i+=3)
	{
		bool isInsert = false;
		if (pos == vIntTriInx.end())
		{
			isInsert = true;
		}
		if (i != *pos)
		{
			isInsert = true;
		}
		else
		{
			++pos;
		}

		if (isInsert)
		{
			vInx.push_back(vTerrianInx[i + 0]);
			vInx.push_back(vTerrianInx[i + 1]);
			vInx.push_back(vTerrianInx[i + 2]);
		}
	}
}
#endif

void RoadTerrian::SaveBinaray(CSaveData& fp)
{
	fp.Write(m_Triangle.tds().number_of_vertices());
	map<CDTP::Vertex_handle, int> mapVerInx;
	int inxVer = 0;
	CDTP::Vertex_handle verFinite = m_Triangle.infinite_vertex();
	mapVerInx[verFinite] = inxVer++;
	for (CDTP::Vertex_iterator vit = m_Triangle.vertices_begin(); vit != m_Triangle.vertices_end(); ++vit)
	{
		if (vit->handle() != verFinite)
		{
			mapVerInx[vit->handle()] = inxVer++;
			WritePoint(fp,vit->point());
		}
	}

	fp.Write(m_Triangle.tds().number_of_full_dim_faces());

	map<CDTP::Face_handle, int> mapFaceInx;
	int inxFace = 0;
	for (CDTP::All_faces_iterator ib = m_Triangle.tds().face_iterator_base_begin(); ib != m_Triangle.tds().face_iterator_base_end(); ++ib)
	{
		mapFaceInx[ib] = inxFace++;
		for (int j = 0; j < 3; ++j)
		{
			fp.Write(mapVerInx[ib->vertex(j)]);
		}
	}
	// neighbor pointers of the  faces
	for (CDTP::All_faces_iterator it = m_Triangle.tds().face_iterator_base_begin(); it != m_Triangle.tds().face_iterator_base_end(); ++it)
	{
		for (int j = 0; j < 3; ++j)
		{
			fp.Write(mapFaceInx[it->neighbor(j)]);
		}
	}

	// write constrained status
	for (CDTP::All_faces_iterator ib = m_Triangle.tds().face_iterator_base_begin(); ib != m_Triangle.tds().face_iterator_base_end(); ++ib)
	{
		for (int j = 0; j < 3; ++j)
		{
			if (ib->is_constrained(j)) { fp.Write(1); }
			else { fp.Write(0); }
		}
	}

#ifdef USE_CONSTRAINT_TRIANGLE
	fp.Write(m_idUnitPolyInxMax);
	//保存合并多边形数据
	fp.Write(m_mapUnitAndID.size());
	for (map<int, suUnitPolyInfo>::iterator pos = m_mapUnitAndID.begin(); pos != m_mapUnitAndID.end(); ++pos)
	{
		suUnitPolyInfo& info = pos->second;
		fp.Write(info.m_idPolyUnit);

		fp.Write(info.m_vInxPolyInfo.size());
		fp.Write(info.m_vInxPolyInfo.begin(), info.m_vInxPolyInfo.end());
		
		fp.Write(info.m_idConstrain.size());
		for (CDTP::Constraint_id idCon : info.m_idConstrain)
		{
			vector<int> vInx;
			for (CDTP::Vertices_in_constraint verPos = m_Triangle.vertices_in_constraint_begin(idCon);
				verPos != m_Triangle.vertices_in_constraint_end(idCon);
				++verPos
				)
			{
				vInx.push_back(mapVerInx[(*verPos)->handle()]);
			}

			fp.Write((int)vInx.size());
			fp.Write(vInx.begin(), vInx.end());

		}


	}
	
	//保存多边形数据
	fp.Write(m_vConstrainPolyGroup.size());
	for (map<int, ZMVector<suConstrainPolyInfo>>::iterator pos = m_vConstrainPolyGroup.begin(); pos != m_vConstrainPolyGroup.end(); ++pos)
	{
		fp.Write(pos->first);
		ZMVector<suConstrainPolyInfo>& vInfo = pos->second;
		fp.Write(vInfo.size());
		for (suConstrainPolyInfo info : vInfo)
		{
			fp.Write(info.m_idConstrain);
			fp.Write(info.m_idPolyUnit);
			Polygon_2 &poly = info.m_poly;
			fp.Write(poly.size());
			for (Polygon_2::Vertex_const_iterator pos = poly.vertices_begin(); pos != poly.vertices_end(); ++pos)
			{
				fp.Write(ZMPoint3D(*pos));
			}
		}

	}
#endif
}

void RoadTerrian::LoadBinaray(CLoadData& fp)
{
	ClearALLData();
	//CDTP m_Triangle1;

	m_Triangle.tds().clear();
	m_Triangle = CDTP();


	int iNumVer, iNumFace;
	fp.Read(iNumVer);
	std::vector<CDTP::Vertex_handle > V(iNumVer);
	ZMPoint3D pntOrign(XMIN, YMIN, 0.0f);
	V[0] = m_Triangle.infinite_vertex();
	m_Triangle.tds().set_dimension(2);

	//{
	//	CDTP::Face_handle fInfi = m_Triangle.infinite_face();
	//	CDTP::Vertex_handle v = m_Triangle.infinite_vertex();
	//	int inx = fInfi->index(v);
	//	fInfi->neighbor(inx);
	//}
	for (int i = 1; i < iNumVer; ++i)
	{
		V[i] = m_Triangle.tds().create_vertex();
		Point pnt;
		ReadPoint(fp, pnt);
		//pwidget->SetCamera(ZMPoint3D(505735.0532, 3242693.238, 324.2035f), ZMPoint3D(505731.7455f, 3242712.9625f, 324.20f));


		pnt.Index() = 1;
		V[i]->set_point(pnt);
	}
	fp.Read(iNumFace);
	std::vector<CDTP::Face_handle> F(iNumFace);
	for (int i = 0; i < iNumFace; ++i)
	{
		F[i] = m_Triangle.tds().create_face();
		for (int j = 0; j < 3; ++j)
		{
			int inx;
			fp.Read(inx);
			F[i]->set_vertex(j, V[inx]);
			// The face pointer of vertices is set too often,
			// but otherwise we had to use a further map
			V[inx]->set_face(F[i]);
		}
	}
	for (int i = 0; i < iNumFace; ++i)
	{
		for (int j = 0; j < 3; ++j) 
		{
			int inx;
			fp.Read(inx);
			F[i]->set_neighbor(j, F[inx]);
		}
	}

	for (CDTP::All_faces_iterator fit = m_Triangle.tds().face_iterator_base_begin();
		fit != m_Triangle.tds().face_iterator_base_end();  ++fit)
	{
		int c[3];
		fp.Read(c[0]);
		fp.Read(c[1]);
		fp.Read(c[2]);
		for (int k = 0; k < 3; ++k)
		{
			fit->set_constraint(k, c[k] == 1);

		}
	}

	m_FaceCatch = CDTP::Face_handle();
	return;
	int idUnitPolyInxMax;
	fp.Read(idUnitPolyInxMax);
#ifdef USE_CONSTRAINT_TRIANGLE
	m_idUnitPolyInxMax = max(idUnitPolyInxMax, m_idUnitPolyInxMax);

	int iUnitPolyNum;
	fp.Read(iUnitPolyNum);
	for (int i = 0; i < iUnitPolyNum; ++i)
	{
		suUnitPolyInfo info;
		fp.Read(info.m_idPolyUnit);

		int iNum;
		fp.Read(iNum); 
		info.m_vInxPolyInfo.resize(iNum);
		fp.Read(info.m_vInxPolyInfo.begin(), info.m_vInxPolyInfo.end());

		int iNumOfConstrain;
		fp.Read(iNumOfConstrain);
		for (int j=0; j<iNumOfConstrain; ++j)
		{
			set<int> vContraPnt;
			vector<int> vInx;
			int iContraintPntNum;
			fp.Read(iContraintPntNum);
			vInx.resize(iContraintPntNum);
			fp.Read(vInx.begin(), vInx.end());
			if (vInx.size() > 1)
			{
				vContraPnt.insert(vInx.begin(), vInx.end());
				CDTP::Constraint_hierarchy& hierar = m_Triangle.hierarchy_ref();
				CDTP::Constraint_id cid = m_Triangle.insert_constraint(V[vInx[0]], V[vInx[1]]);
				for (int j = 1; j < vInx.size() - 1; ++j)
				{
					hierar.append_constraint(cid, V[vInx[j]], V[vInx[j + 1]]);
					m_Triangle.insert_subconstraint(V[vInx[j]], V[vInx[j + 1]]);
				}
				info.m_idConstrain.push_back(cid);
			}
			m_mapUnitAndID[info.m_idPolyUnit] = info;
		}
		
	}

	int iPolyNum;
	fp.Read(iPolyNum);
	for (int i = 0; i < iUnitPolyNum; ++i)
	{
		int idRoad;
		fp.Read(idRoad);
		int iNum;;
		fp.Read(iNum);
		ZMVector<suConstrainPolyInfo> vInfo(iNum);
		for (int j=0; j<iNum; ++j)
		{
			suConstrainPolyInfo& info = vInfo[j];
			fp.Read(info.m_idConstrain);
			fp.Read(info.m_idPolyUnit);
			Polygon_2 &poly = info.m_poly;

			int iPntSize;
			fp.Read(iPntSize);
			ZMVector<ZMPoint3D> vPnt(iPntSize);
			fp.Read(vPnt.begin(), vPnt.end());

			ZMVector<Point>  vPntPoly = ToCGALPntData(vPnt);
			for (Point pnt : vPntPoly)
			{
				poly.push_back(pnt);
			}
		}
	}

#endif
	m_FaceCatch = CDTP::Face_handle();

	//重新计算数据
	//for (CDTP::Constraint_iterator pos = m_Triangle.constraints_begin(); pos != m_Triangle.constraints_end(); ++pos)
	//{
	//	m_vConstraintID.push_back(pos->second);
	//	ZMVector<Point> vPntConstraint;
	//	for (CDTP::Points_in_constraint_iterator pntPos = m_Triangle.points_in_constraint_begin(pos->second);
	//		pntPos != m_Triangle.points_in_constraint_end(pos->second);
	//		++pntPos
	//		)
	//	{
	//		vPntConstraint.push_back(*pntPos);
	//	}
	//	m_vContrainPnt.push_back(vPntConstraint);
	//}

	//for (CDTP::Point_iterator pos = m_Triangle.points_begin(); pos != m_Triangle.points_end(); ++pos)
	//{
	//	if (vContraPnt.find(pos->Index()) == vContraPnt.end())
	//	{
	//		m_vPnt.push_back(*pos);
	//	}
	//}
}

void RoadTerrian::LoadAscii(CFileLoadData& fp)
{
	ClearALLData();
	fp >> m_Triangle;
	for (CDTP::Vertex_iterator pos = m_Triangle.vertices_begin(); pos != m_Triangle.vertices_end(); ++pos)
	{
		if (pos->handle() != m_Triangle.infinite_vertex()->handle())
		{
			int inx;
			ZMReal z;
			fp >> inx; fp >> z;
			(*pos).point().z() = z;
			(*pos).point().Index() = inx;

		}
	}
	map<int, CDTP::Vertex_handle> V;
	set<int> vContraPnt;
	for (CDTP::Vertex_iterator pos = m_Triangle.vertices_begin(); pos != m_Triangle.vertices_end(); ++pos)
	{
		V[(*pos).point().Index()] = pos->handle();
	}

	assert(false);
	int iContraintNum;
	fp >> iContraintNum;
	for (int i = 0; i < iContraintNum; ++i)
	{
		vector<int> vInx;
		int iContraintPntNum;
		fp >> iContraintPntNum;
		for (int j = 0; j < iContraintPntNum; ++j)
		{
			int inx;
			fp >> inx;
			vInx.push_back(inx);
			vContraPnt.insert(inx);
		}
		CDTP::Constraint_hierarchy& hierar = m_Triangle.hierarchy_ref();
		CDTP::Constraint_id cid = m_Triangle.insert_constraint(V[vInx[0]], V[vInx[1]]);
		for (int j = 1; j < vInx.size() - 1; ++j)
		{
			hierar.append_constraint(cid, V[vInx[j]], V[vInx[j + 1]]);
			m_Triangle.insert_subconstraint(V[vInx[j]], V[vInx[j + 1]]);
		}
	}
	m_FaceCatch = CDTP::Face_handle();

	/*for (CDTP::Constraint_iterator pos = m_Triangle.constraints_begin(); pos != m_Triangle.constraints_end(); ++pos)
	{
		m_vConstraintID.push_back(pos->second);
		ZMVector<Point> vPntConstraint;
		for (CDTP::Points_in_constraint_iterator pntPos = m_Triangle.points_in_constraint_begin(pos->second);
			pntPos != m_Triangle.points_in_constraint_end(pos->second);
			++pntPos
			)
		{
			vPntConstraint.push_back(*pntPos);
		}
		m_vContrainPnt.push_back(vPntConstraint);
	}

	for (CDTP::Point_iterator pos = m_Triangle.points_begin(); pos != m_Triangle.points_end(); ++pos)
	{
		if (vContraPnt.find(pos->Index()) == vContraPnt.end())
		{
			m_vPnt.push_back(*pos);
		}
	}*/
}

void RoadTerrian::SaveAscii(CFileSaveData& fp) 
{
	fp << m_Triangle;
	//保存点的inx和z值
	for (CDTP::Vertex_iterator pos = m_Triangle.vertices_begin(); pos != m_Triangle.vertices_end(); ++pos)
	{
		if (pos->handle() != m_Triangle.infinite_vertex()->handle())
		{
			Point pnt = (*pos).point();
			fp << pnt.Index() << " " << pnt.z() << "\r\n";
		}
	}
	assert(false);
	////保存限制边数据
	//fp << m_Triangle.number_of_constraints() << "\r\n";
	//for (CDTP::Constraint_id idCon : m_vConstraintID)
	//{
	//	vector<int> vInx;
	//	for (CDTP::Vertices_in_constraint verPos = m_Triangle.vertices_in_constraint_begin(idCon);
	//		verPos != m_Triangle.vertices_in_constraint_end(idCon);
	//		++verPos
	//		)
	//	{
	//		vInx.push_back((*verPos)->point().Index());
	//	}

	//	fp << vInx.size() << "\r\n";
	//	for (int inx : vInx)
	//	{
	//		fp << inx << " ";
	//	}
	//	fp << "\r\n";
	//}
}

void RoadTerrian::Save(QString strFileName, int iSaveType) 
{
#ifdef _TESTPOINT
	ClearALLData();
	ZMPoint3D pnt0, pnt1;
	pnt0 = ZMPoint3D(0, 0, 100);
	pnt0 = ZMPoint3D(1, 1, 101);
	ZMVector<ZMPoint3D> vConstrain;
	vConstrain.push_back(pnt0);
	vConstrain.push_back(pnt1);
	InsertConstrained(vConstrain);
	vConstrain.clear();
	ZMPoint3D pnt;
	vConstrain.push_back(pnt0);
	pnt = ZMPoint3D(5, 2, 102);
	vConstrain.push_back(pnt);
	pnt = ZMPoint3D(6, 0, 103);
	vConstrain.push_back(pnt);
	pnt = ZMPoint3D(3, 0, 104);
	vConstrain.push_back(pnt);
	InsertConstrained(vConstrain);
#endif
	CFileSaveData fp(strFileName);
	if (fp.IsOpen())
	{
		if (iSaveType == 0) SaveBinaray(fp);
		else SaveAscii(fp);
	}
}

void RoadTerrian::Load(QString strFileName, int iSaveType)
{
	CFileLoadData fp(strFileName);
	if (fp.IsOpen())
	{
		if (iSaveType == 0) LoadBinaray(fp);
		else LoadAscii(fp);
#ifndef USE_CONSTRAINT_TRIANGLE
		UpdateTreeData();
#endif
	}
}

void RoadTerrian::WritePolyon(const Polygon_2& polyon, CFileSaveData& fp)
{
	fp << "3dpoly ";
	for (Polygon_2::Vertex_iterator pos = polyon.vertices_begin(); pos != polyon.vertices_end(); ++pos)
	{
		fp << (*pos).x() << ",";		fp << (*pos).y() << ",";		fp << (*pos).z() << "\r\n";

	}
	fp << "\r\n";
}

void RoadTerrian::WriteDataToCAD(ZMString strFileName, const ZMPoint3D& pnt, ZMReal fDis)
{
	if (fDis < 0)
	{
		return;
	}
	UpdatePntAndTriData();
	ZMPoint3D pntOrign = pnt;
	CFileSaveData fp(strFileName.c_str());
	for (CDTP::All_faces_iterator pos = m_Triangle.all_faces_begin(); pos != m_Triangle.all_faces_end(); ++pos)
	{
		ZMPoint3D pntTemp = ZMPoint3D(pos->vertex(0)->point());
		if (fabs(pntTemp[0] - pntOrign[0]) < fDis && fabs(pntTemp[1] - pntOrign[1]) < fDis)
		{
			if (pos->info().nesting_level % 2 == 0)
			{
				bool isValid = true;
				for (int i = 0; i < 3; ++i)
				{
					if (pos->vertex(i)->point().Index() < 0)
					{
						isValid = false;
						break;
					}
				}
				if (isValid)
				{
					ZMPoint3D pnt;
					fp << "3dpoly ";
					for (int i = 0; i < 3; ++i)
					{
						pnt = ZMPoint3D(pos->vertex(i)->point());
						fp << pnt.x() << ",";		fp << pnt.y() << ",";		fp << pnt.z() << "\r\n";
					}
					pnt = ZMPoint3D(pos->vertex(0)->point());
					fp << pnt.x() << ",";		fp << pnt.y() << ",";		fp << pnt.z() << "\r\n";
					fp << "\r\n";
				}
			}
			
		}
		
	}
	
}

#ifdef DEBUG
void RoadTerrian::SimpleData()
{
	ZMPoint3D pntOrign(505735.0532, 3242693.238, 324.2035f);
	CDTP::Vertex_handle verFinite = m_Triangle.infinite_vertex();
	ZMVector<ZMPoint3D> vPnt;
	for (CDTP::Vertex_iterator vit = m_Triangle.vertices_begin(); vit != m_Triangle.vertices_end(); ++vit)
	{
		if (vit->handle() != verFinite)
		{
			ZMPoint3D pntTemp = ZMPoint3D(vit->point());
			if (fabs(pntTemp[0] - pntOrign[0])< 1000 && fabs(pntTemp[1] - pntOrign[1])< 1000)
			{
				vPnt.push_back(pntTemp);
			}
		}
	}

	{
		CFileSaveData fp("pntSimple.txt");
		for (ZMPoint3D pnt : vPnt)
		{
			char buffer[1024];
			sprintf_s(buffer, "%f	%f	%f\n", pnt[0], pnt[1], pnt[2]);
			fp << buffer;
		}
	}

	{
		vPnt.clear();
		for (CDTP::Vertex_iterator vit = m_Triangle.vertices_begin(); vit != m_Triangle.vertices_end(); ++vit)
		{
			if (vit->handle() != verFinite)
			{
				ZMPoint3D pntTemp = ZMPoint3D(vit->point());
				vPnt.push_back(pntTemp);
			}
		}

		CFileSaveData fp("pntAll.txt");
		for (ZMPoint3D pnt : vPnt)
		{
			char buffer[1024];
			sprintf_s(buffer, "%f	%f	%f\n", pnt[0], pnt[1], pnt[2]);
			fp << buffer;
		}
	}

}
#endif


#endif